///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_IMAGE_ALGORITHM_CALIBRATION_H_
#define _BUOLA_IMAGE_ALGORITHM_CALIBRATION_H_

#include <buola/image/algorithm/detail/opencv.h>
#include <buola/image/algorithm/transform.h>
#include <buola/mat/cquat.h>

namespace buola { namespace img {

//! \name camera calibration
//@{

class CCameraParameters
{
public:
    void SetMatrix(const CPoint_d &pF,const CPoint_d &pC)
    {
        mMatrix[0]=pF.x;
        mMatrix[1]=0;
        mMatrix[2]=pC.x;
        mMatrix[3]=0;
        mMatrix[4]=pF.y;
        mMatrix[5]=pC.y;
        mMatrix[6]=0;
        mMatrix[7]=0;
        mMatrix[8]=1;
    }

    CPoint_d GetF() const     {   return CPoint_d(mMatrix[0],mMatrix[4]);   }
    CPoint_d GetC() const     {   return CPoint_d(mMatrix[2],mMatrix[5]);   }

    void SetDistortion(double k1=0,double k2=0,double p1=0,double p2=0)
    {
        mDistortion[0]=k1;
        mDistortion[1]=k2;
        mDistortion[2]=p1;
        mDistortion[3]=p2;
    }

    mat::CVec3d PixelToImage(const CPoint_d &pP) const
    {
        return mat::CVec3d((pP.x-mMatrix[2])/mMatrix[0],
                      (pP.y-mMatrix[5])/mMatrix[4],1);
    }

    CPoint_d ImageToPixel(const mat::CVec3d &pP) const
    {
        return CPoint_d(pP.x()/pP.z()*mMatrix[0]+mMatrix[2],pP.y()/pP.z()*mMatrix[4]+mMatrix[5]);
    }

    double mMatrix[9];
    double mDistortion[4];
};

class CStereoParameters
{
public:
    CCameraParameters mLeftInt;
    CCameraParameters mRightInt;

    mat::CVec3d mTrans;
    mat::CQuat_d mRotation;
    double mFundamental[9];

    CSize_i mWarpSize;

    double mLeftCoeffs[3][3];
    double mRightCoeffs[3][3];
};

inline io::CTextWriter &operator<<(io::CTextWriter &pWriter,const CCameraParameters &pParams)
{
    for(int i=0;i<9;i++)
        pWriter << pParams.mMatrix[i] << " ";
    for(int i=0;i<4;i++)
        pWriter << pParams.mDistortion[i] << " ";
    return pWriter;
}

inline io::CTextReader &operator>>(io::CTextReader &pReader,CCameraParameters &pParams)
{
    for(int i=0;i<9;i++)
        pReader >> pParams.mMatrix[i];
    for(int i=0;i<4;i++)
        pReader >> pParams.mDistortion[i];
    return pReader;
}

inline io::CTextWriter &operator<<(io::CTextWriter &pWriter,const CStereoParameters &pParams)
{
    pWriter<< pParams.mLeftInt << "\n";
    pWriter<< pParams.mRightInt << "\n";
    pWriter<< pParams.mTrans.x() << " " << pParams.mTrans.y() << " " <<
            pParams.mTrans.z() << " ";
    pWriter << pParams.mRotation.w << " " << pParams.mRotation.x << " " << 
          pParams.mRotation.y << " " << pParams.mRotation.z << " ";
    for(int i=0;i<9;i++)
    {
        pWriter<< pParams.mFundamental[i] << " ";
    }
    pWriter<< pParams.mWarpSize.x << " "<< pParams.mWarpSize.y << " ";
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            pWriter<< pParams.mLeftCoeffs[i][j] << " ";
        }
    }
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            pWriter<< pParams.mRightCoeffs[i][j] << " ";
        }
    }
    
    return pWriter;
}

template<typename tChar>
inline io::CTextReader &operator>>(io::CTextReader &pReader,CStereoParameters &pParams)
{
    pReader >> pParams.mLeftInt;
    pReader >> pParams.mRightInt;
    pReader >> pParams.mTrans.x() >> pParams.mTrans.y() >> pParams.mTrans.z();
    pReader >> pParams.mRotation.w >> pParams.mRotation.x >> 
          pParams.mRotation.y >> pParams.mRotation.z;
    for(int i=0;i<9;i++)
    {
        pReader >> pParams.mFundamental[i];
    }
    pReader >> pParams.mWarpSize.x >> pParams.mWarpSize.y;
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            pReader >> pParams.mLeftCoeffs[i][j];
        }
    }
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            pReader >> pParams.mRightCoeffs[i][j];
        }
    }

    return pReader;
}

template<typename tFormat>
inline bool find_chessboard_corners(const CView<tFormat> &pSrc,int pW,int pH,
              std::vector<CPoint_d> &pCorners,int pFlags)
{
    std::vector<cv::Point2f> lCorners;
    
    bool lOk=cv::findChessboardCorners(mat_wrap(pSrc),cv::Size(pW,pH),lCorners,pFlags);

    pCorners.resize(lCorners.size());

    for(int i=0;i<lCorners.size();i++)
        pCorners[i]={lCorners[i].x,lCorners[i].y};

    return lOk;
}

template<typename tFormat>
inline void undistort(const CView<tFormat> &pSrc,const CMView<tFormat> &pDst,
            CCameraParameters &pParams)
{
    cv::Mat lMatrix(3,3,CV_64FC1,pParams.mMatrix);
    cv::Mat lDistortion(4,1,CV_64FC1,pParams.mDistortion);

    cv::undistort(mat_wrap(pSrc),mat_wrap(pDst),lMatrix,lDistortion,lMatrix);
}

inline void init_undistort_map(CMap &pMap,CCameraParameters &pParams)
{
    cv::Mat lMatrix(3,3,CV_64FC1,pParams.mMatrix);
    cv::Mat lDistortion(4,1,CV_64FC1,pParams.mDistortion);

    cv::initUndistortRectifyMap(lMatrix,lDistortion,cv::Mat(),lMatrix,cv::Size(pMap.Width(),pMap.Height()),CV_32FC1,
                                mat_wrap(pMap.mX),mat_wrap(pMap.mY));
}

inline void calibrate_camera(std::vector<int> &pViewSizes,
            std::vector<double> &pImgPoints,
            std::vector<double> &pObjPoints,
            const CSize_i &pSize,CCameraParameters &pParams,
            int pFlags=0)
{
    int lTotalPoints=std::accumulate(pViewSizes.begin(),pViewSizes.end(),0);

    CvMat lObjPoints=cvMat(lTotalPoints,1,CV_64FC3,pObjPoints.data());
    CvMat lImgPoints=cvMat(lTotalPoints,1,CV_64FC2,pImgPoints.data());
    CvMat lViewSizes=cvMat(pViewSizes.size(),1,CV_32SC1,pViewSizes.data());
    CvMat lMatrix=cvMat(3,3,CV_64FC1,pParams.mMatrix);
    CvMat lDistortion=cvMat(4,1,CV_64FC1,pParams.mDistortion);

    cvCalibrateCamera2(&lObjPoints,&lImgPoints,&lViewSizes,
              cvSize(pSize.x,pSize.y),&lMatrix,&lDistortion,
              nullptr,nullptr,pFlags);
}

inline void calibrate_camera(std::vector<int> &pViewSizes,
            std::vector<double> &pImgPoints,
            std::vector<double> &pObjPoints,
            const CSize_i &pSize,CCameraParameters &pParams,
            std::vector<mat::CQuat_d> &pRotations,
            std::vector<mat::CVec3d> &pTranslations,
            int pFlags=0)
{
    int lTotalPoints=std::accumulate(pViewSizes.begin(),pViewSizes.end(),0);

    CvMat lObjPoints=cvMat(lTotalPoints,1,CV_64FC3,pObjPoints.data());
    CvMat lImgPoints=cvMat(lTotalPoints,1,CV_64FC2,pImgPoints.data());
    CvMat lViewSizes=cvMat(pViewSizes.size(),1,CV_32SC1,pViewSizes.data());
    CvMat lMatrix=cvMat(3,3,CV_64FC1,pParams.mMatrix);
    CvMat lDistortion=cvMat(4,1,CV_64FC1,pParams.mDistortion);

    CvMat *lRot=cvCreateMat(pViewSizes.size(),3,CV_64FC1);
    CvMat *lTrans=cvCreateMat(pViewSizes.size(),3,CV_64FC1);

    cvCalibrateCamera2(&lObjPoints,&lImgPoints,&lViewSizes,
              cvSize(pSize.x,pSize.y),&lMatrix,&lDistortion,
              lRot,lTrans,pFlags);

    for(int i=0;i<pViewSizes.size();i++)
    {
        mat::CVec3d lVector(lRot->data.db[i*3],lRot->data.db[i*3+1],
                      lRot->data.db[i*3+2]);
        pRotations.push_back(mat::CQuat_d(mat::nCompressedRotation,lVector));

        pTranslations.push_back(mat::CVec3d(
            lTrans->data.db[i*3],lTrans->data.db[i*3+1],lTrans->data.db[i*3+2]));
    }

    cvReleaseMat(&lRot);
    cvReleaseMat(&lTrans);
}

inline void project_points(std::vector<double> &pObjPoints,
          const mat::CQuat_d &pRotation,const mat::CVec3d &pTranslation,
          CCameraParameters &pParams,std::vector<CPoint_d> &pImgPoints)
{
    int lPointCount=pObjPoints.size()/3;

    CvMat lObjPoints=cvMat(lPointCount,1,CV_64FC3,pObjPoints.data());
    CvMat *lImgPoints=cvCreateMat(lPointCount,1,CV_64FC2);

    CvMat lMatrix=cvMat(3,3,CV_64FC1,pParams.mMatrix);
    CvMat lDistortion=cvMat(4,1,CV_64FC1,pParams.mDistortion);

    CvMat *lRot=cvCreateMat(3,1,CV_64FC1);
    CvMat *lTrans=cvCreateMat(3,1,CV_64FC1);

    mat::CVec3d lVector=pRotation.GetCompressedVector();

    lRot->data.db[0]=lVector.x();
    lRot->data.db[1]=lVector.y();
    lRot->data.db[2]=lVector.z();

    lTrans->data.db[0]=pTranslation.x();
    lTrans->data.db[1]=pTranslation.y();
    lTrans->data.db[2]=pTranslation.z();

    cvProjectPoints2(&lObjPoints,lRot,lTrans,&lMatrix,&lDistortion,lImgPoints);

    for(int i=0;i<lPointCount;i++)
    {
        pImgPoints.push_back(CPoint_d(lImgPoints->data.db[i*2],
                                    lImgPoints->data.db[i*2+1]));
    }

    cvReleaseMat(&lImgPoints);
    cvReleaseMat(&lRot);
    cvReleaseMat(&lTrans);
}

inline void calibrate_extrinsic(
            std::vector<double> &pImgPoints,
            std::vector<double> &pObjPoints,
            CCameraParameters &pParams,
            mat::CQuat_d &pRotation,mat::CVec3d &pTranslation)
{
    CvMat lObjPoints=cvMat(pObjPoints.size()/3,1,CV_64FC3,pObjPoints.data());
    CvMat lImgPoints=cvMat(pImgPoints.size()/2,1,CV_64FC2,pImgPoints.data());
    CvMat lMatrix=cvMat(3,3,CV_64FC1,pParams.mMatrix);
    CvMat lDistortion=cvMat(4,1,CV_64FC1,pParams.mDistortion);

    CvMat *lRot=cvCreateMat(3,1,CV_64FC1);
    CvMat *lTrans=cvCreateMat(3,1,CV_64FC1);

    cvFindExtrinsicCameraParams2(&lObjPoints,&lImgPoints,
              &lMatrix,&lDistortion,lRot,lTrans);

    mat::CVec3d lVector(lRot->data.db[0],lRot->data.db[1],
                  lRot->data.db[2]);
    pRotation=mat::CQuat_d(mat::nCompressedRotation,lVector);

    pTranslation={lTrans->data.db[0],lTrans->data.db[1],lTrans->data.db[2]};

    cvReleaseMat(&lRot);
    cvReleaseMat(&lTrans);
}

template<typename tFormat>
inline void rotate_image(const CView<tFormat> &pSrc,const CMView<tFormat> &pDst,
        const mat::CQuat_d &pRot,const CPoint_d &pF,const CPoint_d &pC,
        const CPoint_d &pNewC)
{
    for(int y=0;y<pDst.Height();++y)
    {
        auto lDstIt=pDst.RowBegin(y);

        for(int x=0;x<pDst.Width();++x)
        {
            mat::CVec3d lPp((x-pNewC.x)/pF.x,(y-pNewC.y)/pF.y,1);
            lPp=pRot*lPp;
            lPp/=lPp.z();

            CPoint_d lP2(lPp.x()*pF.x+pC.x,lPp.y()*pF.y+pC.y);

            if(lP2.x<0||lP2.x>pSrc.Width()-1||
                lP2.y<0||lP2.y>pSrc.Height()-1)
                lDstIt[x]={0,0,0};
            else
                lDstIt[x]=bilinear_interp(pSrc,lP2.x,lP2.y);
        }
    }
}

inline void init_rotation_map(CMap &pMap,const mat::CQuat_d &pRot,
        const CPoint_d &pF,const CPoint_d &pC,const CPoint_d &pNewC)
{
    for(int y=0;y<pMap.mX.Height();++y)
    {
        auto lXIt=pMap.mX.Row(y).begin();
        auto lYIt=pMap.mY.Row(y).begin();

        for(int x=0;x<pMap.mX.Width();++x)
        {
            mat::CVec3d lPp((x-pNewC.x)/pF.x,(y-pNewC.y)/pF.y,1);
            lPp=pRot*lPp;
            lPp/=lPp.z();

            CPoint_d lP2(lPp.x()*pF.x+pC.x,lPp.y()*pF.y+pC.y);

            lXIt[x]=(float)lP2.x;
            lYIt[x]=(float)lP2.y;
        }
    }
}

template<typename tFormat>
inline void rectify(const CView<tFormat> &pLeftSrc,const CMView<tFormat> &pLeftDst,
            const CView<tFormat> &pRightSrc,const CMView<tFormat> &pRightDst,
            CStereoParameters &pParams,const CPoint_d &pOffset=CPoint_d(nZero))
{
    mat::CQuat_d lLeftRot(mat::CVec3d(1,0,0),pParams.mTrans);
    mat::CQuat_d lRightRot=conj(pParams.mRotation)*lLeftRot;

    CPoint_d lNewC((pLeftDst.Width()-1)/2.0+pOffset.x,
                (pLeftDst.Height()-1)/2.0+pOffset.y);

    rotate_image(pLeftSrc,pLeftDst,lLeftRot,
            pParams.mLeftInt.GetF(),pParams.mLeftInt.GetC(),lNewC);

    rotate_image(pRightSrc,pRightDst,lRightRot,
            pParams.mRightInt.GetF(),pParams.mRightInt.GetC(),lNewC);
}

template<typename tFormat>
inline void rectify(const CView<tFormat> &pLeftSrc,const CMView<tFormat> &pLeftDst,
            const CView<tFormat> &pRightSrc,const CMView<tFormat> &pRightDst,
            CRectifyMap &pMap)
{
    remap(pLeftSrc,pLeftDst,pMap.mLeft);
    remap(pRightSrc,pRightDst,pMap.mRight);
}

inline void init_rectify_map(CRectifyMap &pMap,
           CStereoParameters &pParams,const CPoint_d &pOffset=CPoint_d(nZero))
{
    mat::CQuat_d lLeftRot(mat::CVec3d(1,0,0),pParams.mTrans);
    mat::CQuat_d lRightRot=conj(pParams.mRotation)*lLeftRot;

    CPoint_d lNewC((pMap.mLeft.mX.Width()-1)/2.0+pOffset.x,
                (pMap.mLeft.mX.Height()-1)/2.0+pOffset.y);

    init_rotation_map(pMap.mLeft,lLeftRot,
            pParams.mLeftInt.GetF(),pParams.mLeftInt.GetC(),lNewC);

    init_rotation_map(pMap.mRight,lRightRot,
            pParams.mRightInt.GetF(),pParams.mRightInt.GetC(),lNewC);

    pMap.mCenter=lNewC;
}

inline void init_optimal_rectify_map(CRectifyMap &pMap,
           const CStereoParameters &pParams)
{
    mat::CQuat_d lLeftRot(mat::CVec3d(1,0,0),pParams.mTrans);
    mat::CQuat_d lRightRot=conj(pParams.mRotation)*lLeftRot;

    CPoint_d lPoint[4];

    lPoint[0]={0,0};
    lPoint[1]=CPoint_d(pParams.mWarpSize.x,0);
    lPoint[2]=CPoint_d(pParams.mWarpSize.x,pParams.mWarpSize.y);
    lPoint[3]=CPoint_d(0,pParams.mWarpSize.y);

    CPoint_d lF(pParams.mLeftInt.GetF());
    CPoint_d lC(pParams.mLeftInt.GetC());
    mat::CQuat_d lRot=conj(lLeftRot);

    CRect_d lRect;

    for(int i=0;i<4;i++)
    {
        mat::CVec3d lPp((lPoint[i].x-lC.x)/lF.x,(lPoint[i].y-lC.y)/lF.y,1);
        lPp=lRot*lPp;
        lPp/=lPp.z();

        CPoint_d lP2(lPp.x()*lF.x,lPp.y()*lF.y);

        if(i==0)
        {
            lRect={lP2.x,lP2.y,lP2.x,lP2.y};
        }
        else
        {
            lRect.l=std::min(lRect.l,lP2.x);
            lRect.t=std::min(lRect.t,lP2.y);
            lRect.r=std::max(lRect.r,lP2.x);
            lRect.b=std::max(lRect.b,lP2.y);
        }
    }

    lF=pParams.mRightInt.GetF();
    lC=pParams.mRightInt.GetC();
    lRot=conj(lRightRot);

    for(int i=0;i<4;i++)
    {
        mat::CVec3d lPp((lPoint[i].x-lC.x)/lF.x,(lPoint[i].y-lC.y)/lF.y,1);
        lPp=lRot*lPp;
        lPp/=lPp.z();

        CPoint_d lP2(lPp.x()*lF.x,lPp.y()*lF.y);

        lRect.l=std::min(lRect.l,lP2.x);
        lRect.t=std::min(lRect.t,lP2.y);
        lRect.r=std::max(lRect.r,lP2.x);
        lRect.b=std::max(lRect.b,lP2.y);
    }

    pMap.Recreate({((int)lRect.w()+3)/4*4,((int)lRect.h()+3)/4*4});

    CPoint_d lNewC=-lRect.TopLeft();

    init_rotation_map(pMap.mLeft,lLeftRot,
            pParams.mLeftInt.GetF(),pParams.mLeftInt.GetC(),lNewC);

    init_rotation_map(pMap.mRight,lRightRot,
            pParams.mRightInt.GetF(),pParams.mRightInt.GetC(),lNewC);

    pMap.mCenter=lNewC;
}

//@}
    
/*namespace image*/ } /*namespace buola*/ }

#endif
